#ifndef CO_LOCATION_NODE_HPP
#define CO_LOCATION_NODE_HPP

#include <rclcpp/rclcpp.hpp>
#include "client_interfaces/msg/estimate_target_position.hpp"
#include "client_interfaces/msg/config_target.hpp"
#include "client_interfaces/srv/create_new_airplane.hpp"
#include "client_interfaces/srv/delete_airplane.hpp"
#include "client_interfaces/srv/delete_target.hpp"
#include "client_interfaces/srv/change_payload.hpp"
#include "client_interfaces/srv/get_airplanes_id.hpp"
#include "client_interfaces/srv/get_targets_id.hpp"
#include "cpp_co_location_client/Co_Location_Computer.hpp"
#include "yolo_detect_message/msg/geolocation.hpp"
#include "cpp_co_location_client/global_functions.hpp"

class CoLocationNode : public rclcpp::Node
{
public:
    CoLocationNode();
    
private:
    uint8_t DataNum = 0;
    Co_Location_Computer AI_Co_Location_Computer;

    GlobalPosition GlobalOrigin_;
    bool origin_inited_ = false;
    // Timer and publisher for target location estimation
    void publish_estimates();
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<client_interfaces::msg::EstimateTargetPosition>::SharedPtr publisher_;

    // Subscriber callback for "target found" topic
    rclcpp::Subscription<client_interfaces::msg::ConfigTarget>::SharedPtr subscription_;
    void target_callback(const client_interfaces::msg::ConfigTarget::SharedPtr message);
    

    rclcpp::Subscription<yolo_detect_message::msg::Geolocation>::SharedPtr origin_subscription_;
    void init_pos_callback(const yolo_detect_message::msg::Geolocation::SharedPtr message);

    // Service callbacks
    void create_airplane(const std::shared_ptr<client_interfaces::srv::CreateNewAirplane::Request> request,
                         std::shared_ptr<client_interfaces::srv::CreateNewAirplane::Response> response);
    void delete_airplane(const std::shared_ptr<client_interfaces::srv::DeleteAirplane::Request> request,
                         std::shared_ptr<client_interfaces::srv::DeleteAirplane::Response> response);
    void delete_target(const std::shared_ptr<client_interfaces::srv::DeleteTarget::Request> request,
                       std::shared_ptr<client_interfaces::srv::DeleteTarget::Response> response);
    void change_payload(const std::shared_ptr<client_interfaces::srv::ChangePayload::Request> request,
                        std::shared_ptr<client_interfaces::srv::ChangePayload::Response> response);
    void get_airplanes_id(const std::shared_ptr<client_interfaces::srv::GetAirplanesId::Request> request,
                          std::shared_ptr<client_interfaces::srv::GetAirplanesId::Response> response);
    void get_targets_id(const std::shared_ptr<client_interfaces::srv::GetTargetsId::Request> request,
                        std::shared_ptr<client_interfaces::srv::GetTargetsId::Response> response);

    // Service servers
    rclcpp::Service<client_interfaces::srv::CreateNewAirplane>::SharedPtr service_create_airplane_;
    rclcpp::Service<client_interfaces::srv::DeleteAirplane>::SharedPtr service_delete_airplane_;
    rclcpp::Service<client_interfaces::srv::DeleteTarget>::SharedPtr service_delete_target_;
    rclcpp::Service<client_interfaces::srv::ChangePayload>::SharedPtr service_change_payload_;
    rclcpp::Service<client_interfaces::srv::GetAirplanesId>::SharedPtr service_get_airplanes_id_;
    rclcpp::Service<client_interfaces::srv::GetTargetsId>::SharedPtr service_get_targets_id_;
};

#endif // CO_LOCATION_NODE_HPP
